import os
from launch import LaunchDescription
from launch.actions import (
    ExecuteProcess,
    DeclareLaunchArgument,
    RegisterEventHandler,
    LogInfo,
)
from launch.event_handlers import OnProcessStart
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackagePrefix
package_name = 'semantic_nav'


def generate_launch_description():
    bag_name = LaunchConfiguration('bag_name')
    declare_bag_name_cmd = DeclareLaunchArgument(
        'bag_name',
        default_value='test_bag',
        description='The bag file name',
    )
    data_stream_get_node = Node(
        package=package_name,
        executable='data_stream_get',
        name='data_stream_get',
        output='screen'
    )
    # Set the topics to record
    topics_to_play = [
        '/camera_image_color',
        '/camera_image_depth',
        '/camera/color/image_raw',
        '/camera/depth/image_raw',
    ]
    bag_play_cmd = ['ros2', 'bag', 'play', PathJoinSubstitution([FindPackagePrefix('semantic_nav'), 'bag', bag_name]), '--topics'] + topics_to_play
    bag_play_process = ExecuteProcess(
        cmd=bag_play_cmd,
        output='screen'
    )

    return LaunchDescription([
        declare_bag_name_cmd,
        data_stream_get_node,
        RegisterEventHandler(
            OnProcessStart(
                target_action=data_stream_get_node,
                on_start=[
                    LogInfo(msg='data stream get node start'),
                    bag_play_process
                ]
            )
        )
    ])


if __name__ == '__main__':
    generate_launch_description()
